(*AMBER*)
SetDirectory[
  "/home/shu/workspace/Research/AMBER_Output_Test/model"];
<< RobotLinks.m
<< Linearize.m
(*SetDirectory["/home/shu/workspace/Research/AMBER_Output_Test/model/\
"<>"build_torso"];*)
(*LineNumber = 2;*)
IndexAllPre = Import["/home/shu/workspace/Research/AMBER_Output_Test/data/IndexAll.mat", "MAT"];
(*Dimensions[IndexAllPre];*)
IndexAll = Join[First[IndexAllPre]];
(*Dimensions[IndexAll];*)

HipPosIndex = Round[IndexAll[[LineNumber, 1]]];
NSslopeIndex = Round[IndexAll[[LineNumber, 2]]];
TorsoIndex = Round[IndexAll[[LineNumber, 3]]];
t0 = AbsoluteTime[];
(*model specification*)

robotInfo = {{Lc, Lt, LT, rc, rt, rT, mh, mt, mc, g},
   {347.98, 261.112, 0, 282.37, 128.15, 9.97, 804.83, 606.15, 213.79, 
    9810}};
constsubs = (#1[[1]] -> Rationalize[#1[[2]]/1000] &) /@ 
   Transpose[robotInfo];
ndof = 5;
mm = {mc, mt, mh, mt, mc} /. constsubs;
statesubs = 
  Join[Table[Subscript[\[Theta], i][t] -> x[i], {i, 1, ndof}], 
   Table[Derivative[1][Subscript[\[Theta], i]][t] -> x[i + ndof], {i, 
     1, ndof}]];
(*define inertia matrices*)

For[i = 1, i <= Length[mm], i++, 
  Subscript[\[ScriptCapitalM], i] = mm[[i]] DiagonalMatrix[{1, 1, 1}]];
(*add inertia for torso (2.5 cm radius),the x,z terms will not show \
up in the EOM for this 2d model*)

Subscript[\[ScriptCapitalI], 1] = Rationalize[( {
      {1967374.33, 0, 12.37},
      {0, 1946798.09, -19.96},
      {12.37, -19.96, 119696.29}
     } )/1000000000, 1/1000000000];
Subscript[\[ScriptCapitalI], 2] = Rationalize[( {
      {6494948.89, 58.80, 2024.23},
      {58.80, 6396011.40, 146697.91},
      {2024.23, 146697.91, 418371.07}
     } )/1000000000, 1/1000000000];
Subscript[\[ScriptCapitalI], 3] = Rationalize[( {
      {3730232.81, -7.44, 1948.96},
      {-7.44, 518271.83, -1348.28},
      {1948.96, -1348.28, 3577190.08}
     } )/1000000000, 1/1000000000];
Subscript[\[ScriptCapitalI], 4] = Rationalize[( {
      {6494948.89, -60.11, 1985.63},
      {-60.11, 6396011.38, -146697.91},
      {1985.63, -146697.91, 418371.06}
     } )/1000000000, 1/1000000000];
Subscript[\[ScriptCapitalI], 5] = Rationalize[( {
      {1967374.33, 0, -12.37},
      {0, 1946798.091, 19.96},
      {-12.37, 19.96, 119696.29}
     } )/1000000000, 1/1000000000];
For[i = 1, i <= 5, i++, 
 Subscript[\[ScriptCapitalM], i] = 
  Join[Join[Subscript[\[ScriptCapitalM], i], Table[0, {3}, {3}], 2], 
   Join[Table[0, {3}, {3}], Subscript[\[ScriptCapitalI], i], 2], 1]]
p0 = {Subscript[p, x][t] -> 0, Subscript[p, z][t] -> 0, 
   Subscript[p, x]'[t] -> 0, Subscript[p, z]'[t] -> 0};
(*generalized coordinates*)

q = Table[{Subscript[\[Theta], i][t]}, {i, 1, ndof}];
dq = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]q\);
ddq = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]dq\);
qe = Join[{{Subscript[p, x][t]}, {Subscript[p, z][t]}}, q];
dqe = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]qe\);
(*location and direction of twists*)

Subscript[\[Xi], 0] = {0, 0, 0, 0, 0, 0};
Subscript[\[Xi], px] = 
  Simplify[PrismaticTwist[{0, 0, 0}, {1, 0, 0}] /. constsubs];
Subscript[\[Xi], pz] = 
  Simplify[PrismaticTwist[{0, 0, 0}, {0, 0, 1}] /. constsubs];
Subscript[\[Xi], q1] = 
  Simplify[RevoluteTwist[{0, 0, 0}, {0, -1, 0}] /. constsubs];
Subscript[\[Xi], q2] = 
  Simplify[RevoluteTwist[{0, 0, Lc}, {0, -1, 0}] /. constsubs];
Subscript[\[Xi], q3] = 
  Simplify[RevoluteTwist[{0, 0, Lc + Lt}, {0, -1, 0}] /. constsubs];
Subscript[\[Xi], q4] = 
  Simplify[RevoluteTwist[{0, 0, Lc + Lt}, {0, 1, 0}] /. constsubs];
Subscript[\[Xi], q5] = 
  Simplify[RevoluteTwist[{0, 0, Lc}, {0, 1, 0}] /. constsubs];
(*base configuration*)

Subscript[g, Subscript[sl, 1]][0] = 
  Simplify[RPToHomogeneous[IdentityMatrix[3], {0, -33/100000, rc}] /. 
    constsubs];
Subscript[g, Subscript[sl, 2]][0] = 
  Simplify[RPToHomogeneous[
     IdentityMatrix[3], {2/100000, -381/100000, Lc + rt}] /. 
    constsubs];
Subscript[g, Subscript[sl, 3]][0] = 
  Simplify[RPToHomogeneous[
     IdentityMatrix[3], {-24/100000, -4/100000, Lc + Lt + rT}] /. 
    constsubs];
(*Subscript[g, Subscript[sl, \
4]][0]=Simplify[RPToHomogeneous[IdentityMatrix[3],{-24/100000,-4/\
100000,Lc+Lt+Lb}]/.constsubs]*)

Subscript[g, Subscript[sl, 4]][0] = 
  Simplify[RPToHomogeneous[
     IdentityMatrix[3], {2/100000, 381/100000, Lc + rt}] /. constsubs];
Subscript[g, Subscript[sl, 5]][0] = 
  Simplify[RPToHomogeneous[IdentityMatrix[3], {0, 33/100000, rc}] /. 
    constsubs];
Subscript[\[ScriptCapitalJ], 1] = 
  Simplify[BodyJacobian[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 1]][0]]];
Subscript[\[ScriptCapitalJ], 2] = 
  Simplify[BodyJacobian[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 2]][0]]];
Subscript[\[ScriptCapitalJ], 3] = 
  Simplify[BodyJacobian[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 3]][0]]];
Subscript[\[ScriptCapitalJ], 4] = 
  Simplify[
   BodyJacobian[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], 0], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 4]][0]]];
Subscript[\[ScriptCapitalJ], 5] = 
  Simplify[BodyJacobian[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 5]][0]]];
(*calculate the forward kinematics maps*)

Subscript[g, 1][\[Theta]] = 
  Simplify[
   ForwardKinematics[{Subscript[\[Xi], px], 
      Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
      Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, 
     Subscript[g, Subscript[sl, 1]][0]] /. constsubs];
Subscript[g, 2][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
      Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
      Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, 
     Subscript[g, Subscript[sl, 2]][0]] /. constsubs];
Subscript[g, 3][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
      Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
      Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
      Subscript[\[Theta], 3][t]}, 
     Subscript[g, Subscript[sl, 3]][0]] /. constsubs];
Subscript[g, 4][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
      Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
      Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
      Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
      Subscript[\[Theta], 4][t]}, 
     Subscript[g, Subscript[sl, 4]][0]] /. constsubs];
Subscript[g, 5][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
      Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
      Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
      Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
      Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
      Subscript[\[Theta], 5][t]}, 
     Subscript[g, Subscript[sl, 5]][0]] /. constsubs];
Subscript[g, stf][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}] /. constsubs;
Subscript[g, stk][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc}] /. constsubs;
Subscript[g, hip][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + Lt}] /. constsubs;
Subscript[g, torso][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + Lt + rT}] /. 
   constsubs;
Subscript[g, nsk][0] = 
 RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc}] /. constsubs; 
Subscript[g, nsf][0] = 
 RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}] /. constsubs;
Subscript[g, stf][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], 0], 0}, 
     Subscript[g, stf][0]] /. constsubs];
Subscript[g, stk][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, Subscript[g, stk][0]] /. 
    constsubs];
Subscript[g, hip][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, Subscript[g, hip][0]] /. 
    constsubs];
Subscript[g, torso][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
      Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
      Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
      Subscript[\[Theta], 3][t]}, Subscript[g, torso][0]] /. 
    constsubs];
Subscript[g, nsk][\[Theta]] = 
 Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, Subscript[g, nsk][0]] /. constsubs]; 
Subscript[g, nsf][\[Theta]] = 
 Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, nsf][0]] /. constsubs];
(*aniplot*)

pos = Simplify[
   Join[Subscript[g, stf][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, stk][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, hip][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, torso][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, hip][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, nsk][\[Theta]][[{1, 3}, {4}]], 
    Subscript[g, nsf][\[Theta]][[{1, 3}, {4}]], 2]];

(*calculate center of mass*)
Subscript[p, COM] = (\!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[
         i]]\ \(
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]\)[[1, 
          4]]/\((Total[mm])\)\)\)) /. constsubs //. p0 // Simplify;
(*calculate the manipulator inertia matrix*)
\[ScriptCapitalD]e = 
  Simplify[\!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(
\*SubscriptBox[\(\[ScriptCapitalJ]\), \(i\)]\[Transpose]  . 
\*SubscriptBox[\(\[ScriptCapitalM]\), \(i\)] . 
\*SubscriptBox[\(\[ScriptCapitalJ]\), \(i\)]\)\)];
(*project out the generalized coordinates defining the position of \
the stance foot to obtain the reduced \[ScriptCapitalD] matrix*)
\
\[ScriptCapitalD] = 
  Simplify[\[ScriptCapitalD]e[[3 ;; All, 3 ;; All]] /. p0];
(*calculate Coriolis matrix*)
\[ScriptCapitalC] = 
  Simplify[InertiaToCoriolis[\[ScriptCapitalD], Flatten[q], 
    Flatten[dq]]];
(*calculate the potential energy and \[ScriptCapitalG] matrix*)

V = Simplify[g \!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[
        i]]\ \(
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]\)[[3, 4]]\)\) /. constsubs];
\[ScriptCapitalG] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({q, 1}\)]V\)];
(*calculate the \[ScriptCapitalE] matrix for impact and the guard*)
\
(*the position of swing foot*)

Subscript[g, nsf][0] = RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}];
Subscript[g, nsf][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, nsf][0]]];
\[ScriptCapitalE] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[qe], 1}\)]\(\(
\(\*SubscriptBox[\(g\), \(nsf\)]\)[\[Theta]]\)[[{1, 3}, 4]]\)\)];
(*\[ScriptCapitalE]/.{Subscript[\[Theta], 1][t]->.2345,Subscript[\
\[Theta], 2][t]->.1894,Subscript[\[Theta], 3][t]->-.293,Subscript[\
\[Theta], 4][t]->.094,Subscript[\[Theta], 4][t]->-.210,Subscript[\
\[Theta], 5][t]->.923}*)

h = Simplify[Subscript[g, nsf][\[Theta]][[3, 4]] /. p0];
hdot =  Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
     h]\)\)];
(*hip position related*)
(*1 Hip Position*)

HipPos = Subscript[g, hip][\[Theta]][[1, 4]] /. p0;
(*2 Linearized Hip Position*)

LHipPos = 
  Linearize[HipPos, Table[Subscript[\[Theta], i][t], {i, 5}], 
   Table[0, {i, 5}]];
(*non-stance slope related*)
(*1 non-stance slope*)

nsslope = (
   Subscript[g, nsf][\[Theta]][[1, 4]] - 
    Subscript[g, hip][\[Theta]][[1, 4]])/(
   Subscript[g, nsf][\[Theta]][[3, 4]] - 
    Subscript[g, hip][\[Theta]][[3, 4]]) /. p0;
(*2. linearized non-stance slope*)

LinearNSslope = 
  Linearize[nsslope /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
   Table[0, {i, 5}]];
(*3. hip angle*)

HipAngle = Subscript[\[Theta], 3][t] - Subscript[\[Theta], 4][t];
(*torso related*)
(*1 Torso non-stance thigh angle*)

theta4 = Subscript[\[Theta], 4][t];
(*2 Torso Hip angle*)

TorsoHipAngle = 
  Subscript[\[Theta], 1][t] + Subscript[\[Theta], 2][t] + 
   Subscript[\[Theta], 3][t];
(*3 Torso Non-stance Slope*)

nstorso = (
  Subscript[g, nsf][\[Theta]][[1, 4]] - 
   Subscript[g, torso][\[Theta]][[1, 4]])/(
  Subscript[g, nsf][\[Theta]][[3, 4]] - 
   Subscript[g, torso][\[Theta]][[3, 4]]);
(*4 Linearized torso non-stance slope angle*)

LinearNStorso = 
  Linearize[nstorso /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
   Table[0, {i, 5}]];
(*5 Torso stance slope*)

storso = Subscript[g, torso][\[Theta]][[1, 4]]/
  Subscript[g, torso][\[Theta]][[3, 4]];
(*6 Linearized Torso stance slope*)

LinearStorso = 
  Linearize[storso /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
   Table[0, {i, 5}]];
(*7 COM*)
Subscript[p, COM] = (\!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[i]] \(
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]\)[[1, 
          4]]/\((2*\((mc + mt)\) + mh)\)\)\) /. constsubs) //. p0 // 
   Simplify;
(*8 Linearized COM*)
Subscript[Linearp, COM] = 
  Linearize[Subscript[p, COM] /. p0, 
   Table[Subscript[\[Theta], i][t], {i, 5}], Table[0, {i, 5}]];


(*feedback control*)
\[Chi] = Join[q, dq];
d\[Chi] = D[\[Chi], t];
hipOutput = {HipPos, LHipPos};
Subscript[p, hip] = hipOutput[[HipPosIndex]] ;
Subscript[p, hipdot]  = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
\*SubscriptBox[\(p\), \(hip\)]]\)\)];
Subscript[v, hip] = D[Subscript[p, hip], t] // Simplify;
fb = {\[Sigma][t] -> (Subscript[p, hip] - p[1])/
   a[1, 1]};(*time-invariant parameterization:*)
\[Sigma]y = \[Sigma][
   t] /. fb;
HumanFunction[i_] := (
  a[i, 1] Cos[a[i, 2] \[Sigma][t]] + 
   a[i, 3] Sin[a[i, 2] \[Sigma][t]])/Exp[a[i, 4] \[Sigma][t]] + 
  a[i, 5];(*calculate Subscript[y, d]and its derivatives*)

Subscript[y, d, 1] = a[1, 1];
Subscript[y, d, 2] = 
  Transpose[{Table[HumanFunction[i], {i, 2, ndof}]}] /. fb;
Subscript[Dy, d, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(d, 1\)]]\)\)];
Subscript[Dy, d, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(d, 2\)]]\)\)];
Subscript[DLfy, d, 1] = \!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(d, 1\)] . d\[Chi]]\)\) // Simplify;
Subscript[DLfy, d, 2] = \!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(d, 2\)] . d\[Chi]]\)\);
(*calculate actual kinematics outputs and Jacobians*)

Subscript[y, a, 1] = D[Subscript[p, hip], t];
NSslopeOutput = {nsslope, LinearNSslope, HipAngle};
TorsoOutput = {TorsoHipAngle, theta4, nstorso, LinearNStorso, storso, 
   LinearStorso, Subscript[p, COM], Subscript[Linearp, COM]};
Subscript[y, a, 2] = 
  Simplify[{{NSslopeOutput[[NSslopeIndex]]}, {Subscript[\[Theta], 2][
        t]}, {Subscript[\[Theta], 5][
        t]}, {TorsoOutput [[TorsoIndex]]}} /. constsubs /. p0];
Subscript[Dy, a, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(a, 1\)]]\)\)];
Subscript[Dy, a, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(a, 2\)]]\)\)];
Subscript[DLfy, a, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(a, 1\)] . d\[Chi]]\)\)];
Subscript[DLfy, a, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(a, 2\)] . d\[Chi]]\)\)];

(*Inverse Kinematics*)
(*define the reset map*)
\[CapitalDelta] = {{1,
     1, 1, -1, -1},
   {0, 0, 0, 0, 1},
   {0, 0, 0, 1, 0},
   {0, 0, 1, 0, 0},
   {0, 1, 0, 0, 0}};
(*define the guard conditions for pre-and post-impact*)

initialtime = {t -> 0};
(*compute the position of the hip post-impact*)

resetmap = Module[{newq},
   newq = \[CapitalDelta].(q);
   Table[Subscript[\[Theta], i][0] -> newq[[i, 1]], {i, 1, 
     Length[q]}]
   ];
(*Parameterized Time Using the Output of the Hip*)

phipcondition = {p[1] -> Subscript[p, hip]} /. constsubs /. t -> 0;
tau = Simplify[\[Sigma][t] /. fb /. phipcondition /. resetmap /. 
    constsubs];
(*extract the (relative degree two) position-based outputs*)
Subscript[y, 
  2] = (Subscript[y, a, 2]  - Subscript[y, d, 2]  ) /. 
    phipcondition /. constsubs;
(*compute the pre-and post-impact values of the outputs*)
yplus = Subscript[y, 2] /. initialtime /. resetmap // Simplify;
yminus = Subscript[y, 2] /. resetmap;
(*solve for the inverse kinematics*)
(*create a function to solve the \
inverse kinematics*)
SolveEquations[equations_, variables_, solutionnumbers_] :=
  
  Module[{i, solutions = {}, tmpeqn, tmpsolution},
   For[i = 1, i <= Length[variables], i++,
    tmpeqn = (equations[[i]]) == 0;
    tmpsolution = {variables[[
        i]] -> (variables[[i]] /. 
         Flatten[{solutions, 
           Solve[tmpeqn, variables[[i]]][[solutionnumbers[[i]]]]}])};
    solutions = Join[solutions, (tmpsolution /. solutions)]
    ];
   solutions
   ];
yhsolve = 
  Join[Flatten[
     yplus], {h}] /. {Subscript[\[Theta], 3][t] - 
      Subscript[\[Theta], 4][t] -> \[Beta][t]};
(*MatrixForm[yhsolve];*)
MatrixForm[yhsolve[[{2, 3, 1, 5, 4}]]];
Theta5Cond = 
  Solve[yhsolve[[2]] == 0, Subscript[\[Theta], 5][t]][[1]];
Theta2Cond = 
  Solve[yhsolve[[3]] == 0, Subscript[\[Theta], 2][t]][[1]];
Theta1Cond = 
  Solve[yhsolve[[1]] == 0, Subscript[\[Theta], 1][t]][[1]];
ThetabetaCond = Solve[yhsolve[[5]] == 0, \[Beta][t]][[2]];
Theta3Cond = 
  Solve[yhsolve[[4]] == 0, Subscript[\[Theta], 3][t]][[1]];
(*Transpose[{{Subscript[\[Theta], 1][t],Subscript[\[Theta], \
2][t],Subscript[\[Theta], 3][t],Subscript[\[Theta], \
3][t]-\[Beta][t],Subscript[\[Theta], \
5][t]}}]/.Theta3Cond/.atest/.ThetabetaCond/.atest/.Theta1Cond/.atest/.T\
heta5Cond/.atest/.Theta2Cond/.atest;*)

Theta1a = Subscript[\[Theta], 1][t] /. Theta1Cond;
Theta2a = Subscript[\[Theta], 2][t] /. Theta2Cond;
Theta3a = Subscript[\[Theta], 3][t] /. Theta3Cond /. ThetabetaCond;
Theta4a = 
  Subscript[\[Theta], 3][t] - \[Beta][t] /. Theta3Cond /. 
   ThetabetaCond;
Theta5a = Subscript[\[Theta], 5][t] /. Theta5Cond;
(*Velocity of Outputs:Pre and Post Impact*)
(*compute the jacobian of \
the output functions*)Y = Join[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\({
\*SubscriptBox[\(p\), \(hip\)]}\)\), Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(2\)]]\)\)]];
Yplus = Y /. initialtime /. constsubs /. resetmap;
Yminus = Y /. constsubs /. resetmap;
Dh = \!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\({h}\)\);
SetDirectory[
  "/home/shu/workspace/Research/AMBER_Output_Test/model/" <> 
   "buildopt_torso"];
stream = OpenWrite["tau"];
Write[stream, tau /. statesubs];
Close[stream];
Clear[stream];

stream = OpenWrite["y_plus"];
Write[stream, yplus /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["y_minus"];
Write[stream, yminus /. statesubs];
Close[stream];
Clear[stream];

stream = OpenWrite["theta_a1"];
Write[stream, Theta1a /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["theta_a2"];
Write[stream, Theta2a /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["theta_a3"];
Write[stream, Theta3a /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["theta_a4"];
Write[stream, Theta4a /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["theta_a5"];
Write[stream, Theta5a /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["h_dot_minus"];
Write[stream, Dh /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["H_plus"];
Write[stream, Yplus /. statesubs /. constsubs];
Close[stream];
Clear[stream];
stream = OpenWrite["H_minus"];
Write[stream, Yminus /. statesubs];
Close[stream];
Clear[stream];
FolderName = "buildopt_torso_" <> ToString[LineNumber];
(*SetDirectory["/home/shu/workspace/Research/2DKnee_Torso_Outputs"];*)

SetDirectory[
  "/home/shu/workspace/Research/AMBER_Output_Test/model/"];
Run["perl math2matopt_torso2.pl"];
Run["mkdir -p " <> FolderName];
Run["cp ./buildopt_torso/*.m ./" <> FolderName <> "/"];
SetDirectory[
  "/home/shu/workspace/Research/AMBER_Output_Test/model/" <> 
   "build_torso"];
stream = OpenWrite["jpos_mat"];
Write[stream, pos /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["D_mat"];
Write[stream, \[ScriptCapitalD] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["C_mat"];
Write[stream, \[ScriptCapitalC] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["G_vec"];
Write[stream, \[ScriptCapitalG] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["De_mat"];
Write[stream, \[ScriptCapitalD]e /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["E_mat"];
Write[stream, \[ScriptCapitalE] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["h_sca"];
Write[stream, h /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["h_dot_mat"];
Write[stream, hdot /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["phip_sca"];
Write[stream, Subscript[p, hip] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
(*stream=OpenWrite["deltaphip_sca"];
Write[stream,Subscript[deltap, hip]/.constsubs/.statesubs];
Close[stream];
Clear[stream];*)
stream = OpenWrite["phip_dot_mat"];
Write[stream, Subscript[p, hipdot] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
(*stream=OpenWrite["deltaphip_dot_mat"];
Write[stream,Subscript[deltap, hipdot] /.constsubs/.statesubs];
Close[stream];
Clear[stream];*)
stream = OpenWrite["yd1_sca"];
Write[stream, Subscript[y, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["yd2_vec"];
Write[stream, Subscript[y, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dyd1_mat"];
Write[stream, Subscript[Dy, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dyd2_mat"];
Write[stream, Subscript[Dy, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfyd1_mat"];
Write[stream, Subscript[DLfy, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfyd2_mat"];
Write[stream, Subscript[DLfy, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["ya1_sca"];
Write[stream, Subscript[y, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["ya2_vec"];
Write[stream, Subscript[y, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dya1_mat"];
Write[stream, Subscript[Dy, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dya2_mat"];
Write[stream, Subscript[Dy, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfya1_mat"];
Write[stream, Subscript[DLfy, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfya2_mat"];
Write[stream, Subscript[DLfy, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["sigma_sca"];
Write[stream, \[Sigma]y /. constsubs /. statesubs];
Close[stream];
Clear[stream];
FolderName = "build_torso_" <> ToString[LineNumber];
(*SetDirectory["/home/shu/workspace/Research/2DKnee_Torso_Outputs"];*)

SetDirectory[
  "/home/shu/workspace/Research/AMBER_Output_Test/model/"];
Run["perl math2mat_torso.pl"];
Run["mkdir -p " <> FolderName];
Run["cp ./build_torso/*.m ./" <> FolderName <> "/"];
